#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose.hpp>
#include <tf2_eigen/tf2_eigen.hpp>
#include <Eigen/Geometry>
#include <memory>

int main(int argc, char** argv)
{
  // Initialize ROS2
  rclcpp::init(argc, argv);
  auto node = rclcpp::Node::make_shared("matrix_conversion");

  // Create MoveGroupInterface for the manipulator
  auto arm_group = moveit::planning_interface::MoveGroupInterface(node, "zc4");

  // Get current pose
  geometry_msgs::msg::Pose current_pose = arm_group.getCurrentPose().pose;

  // Print position for debugging
  RCLCPP_INFO(node->get_logger(), "Position:");
  RCLCPP_INFO(node->get_logger(), "  x: %f", current_pose.position.x);
  RCLCPP_INFO(node->get_logger(), "  y: %f", current_pose.position.y);
  RCLCPP_INFO(node->get_logger(), "  z: %f", current_pose.position.z);

  // Print orientation for debugging
  RCLCPP_INFO(node->get_logger(), "Orientation (Quaternion):");
  RCLCPP_INFO(node->get_logger(), "  w: %f", current_pose.orientation.w);
  RCLCPP_INFO(node->get_logger(), "  x: %f", current_pose.orientation.x);
  RCLCPP_INFO(node->get_logger(), "  y: %f", current_pose.orientation.y);
  RCLCPP_INFO(node->get_logger(), "  z: %f", current_pose.orientation.z);

  // Convert quaternion to rotation matrix
  Eigen::Quaterniond quaternion(
    current_pose.orientation.w,
    current_pose.orientation.x,
    current_pose.orientation.y,
    current_pose.orientation.z
  );
  Eigen::Matrix3d rotation_matrix = quaternion.toRotationMatrix();

  // Create 4x4 transform matrix
  Eigen::Matrix4d transform_matrix = Eigen::Matrix4d::Identity();
  transform_matrix.block<3,3>(0,0) = rotation_matrix;
  transform_matrix(0,3) = current_pose.position.x;
  transform_matrix(1,3) = current_pose.position.y;
  transform_matrix(2,3) = current_pose.position.z;

  // Print the matrix
  RCLCPP_INFO(node->get_logger(), "4x4 Transform Matrix:");
  RCLCPP_INFO_STREAM(node->get_logger(), transform_matrix);

  // Shutdown ROS2
  rclcpp::shutdown();
  return 0;
}